#include "zf_common_headfile.h"
#include "pid.h"



// Separate PID controllers for angle and angular velocity
PID_typedef AnglePid;
PID_typedef RatePid;
PID_typedef Pid;

void Pid_init(float p, float i, float d, float output_limit, float dead_zone)
{
    Pid.kp = p;
    Pid.ki = i;
    Pid.kd = d;
    Pid.integral = 0;
    Pid.prev_error = 0;
    Pid.output_limit = output_limit;
    Pid.dead_zone = dead_zone;
}
void AnglePid_init(float p, float i, float d, float output_limit, float dead_zone)
{
    AnglePid.kp = p;
    AnglePid.ki = i;
    AnglePid.kd = d;
    AnglePid.integral = 0;
    AnglePid.prev_error = 0;
    AnglePid.output_limit = output_limit;
    AnglePid.dead_zone = dead_zone;
}

void RatePid_init(float p, float i, float d, float output_limit, float dead_zone)
{

    RatePid.kp = p;
    RatePid.ki = i;
    RatePid.kd = d;
    RatePid.integral = 0;
    RatePid.prev_error = 0;
    RatePid.output_limit = output_limit;
    RatePid.dead_zone = dead_zone;
}

int PID_Control(float target, float current)
{
    // Calculate angle error
    static float error,derivative,output;
    error = target - current;
    // Dead zone control
    if(fabs(error) < Pid.dead_zone) {
        error = 0;
    }
    
    // Calculate integral term with anti-windup
    Pid.integral += error;
    if(Pid.integral > Pid.output_limit) Pid.integral = Pid.output_limit;
    if(Pid.integral < -Pid.output_limit) Pid.integral = -Pid.output_limit;
    
    // Calculate derivative term
    derivative = error - Pid.prev_error;
    
    // Calculate PID output
    output = Pid.kp * error +
                   Pid.ki * Pid.integral +
                   Pid.kd * derivative;
    
    // Apply output limits
    if(output > Pid.output_limit) output = Pid.output_limit;
    if(output < -Pid.output_limit) output = -Pid.output_limit;
    
    // Store previous error
    Pid.prev_error = error;
    
    return (int)output;
}

int AnglePID_Control(float target_angle, float current_angle)
{
    // Calculate angle error
    static float error,derivative,output;
    error = target_angle - current_angle;
    // Dead zone control
    if(fabs(error) < AnglePid.dead_zone) {
        error = 0;
    }

    // Calculate integral term with anti-windup
    AnglePid.integral += error;
    if(AnglePid.integral > AnglePid.output_limit) AnglePid.integral = AnglePid.output_limit;
    if(AnglePid.integral < -AnglePid.output_limit) AnglePid.integral = -AnglePid.output_limit;

    // Calculate derivative term
    derivative = error - AnglePid.prev_error;

    // Calculate PID output
    output = AnglePid.kp * error +
                   AnglePid.ki * AnglePid.integral +
                   AnglePid.kd * derivative;

    // Apply output limits
    if(output > AnglePid.output_limit) output = AnglePid.output_limit;
    if(output < -AnglePid.output_limit) output = -AnglePid.output_limit;

    // Store previous error
    AnglePid.prev_error = error;

    return (int)output;
}

int RatePID_Control(float target_rate, float current_rate)
{
    // Calculate rate error

    static float error,derivative,output;
    error = target_rate - current_rate;
    // Dead zone control
    if(fabs(error) < RatePid.dead_zone) {
        error = 0;
    }

    // Calculate integral term with anti-windup
    RatePid.integral += error;
    if(RatePid.integral > RatePid.output_limit) RatePid.integral = RatePid.output_limit;
    if(RatePid.integral < -RatePid.output_limit) RatePid.integral = -RatePid.output_limit;

    // Calculate derivative term
    derivative = error - RatePid.prev_error;

    // Calculate PID output
    output = RatePid.kp * error +
                   RatePid.ki * RatePid.integral +
                   RatePid.kd * derivative;

    // Apply output limits
    if(output > RatePid.output_limit) output = RatePid.output_limit;
    if(output < -RatePid.output_limit) output = -RatePid.output_limit;

    // Store previous error
    RatePid.prev_error = error;

    return (int)output;
}

void set_speed(int16 left_speed,int16 right_speed,int error1,int error2)
{
    if(left_speed>0){left_speed+=error1;}
    if(left_speed<0){left_speed-=error2;}
    if(right_speed>0){right_speed+=error1;}
    if(right_speed<0){right_speed-=error2;}

    small_driver_set_duty(-1*right_speed,left_speed);
}
